import copy
import queue
import random
import math
import gc

import cv2
from bokeh.colors.groups import white
from memory_profiler import profile


class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None
    def __eq__(self, other):
        if not isinstance(other, Node):
            return NotImplemented
        return self.x == other.x and self.y == other.y
    def __hash__(self):
        return hash((self.x, self.y))

# 生成随机点
def get_random_node(width, height):
    return Node(random.randint(0, width), random.randint(0, height))


# 求距离
def distance(node1, node2):
    return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)


# 找到距离最近的节点
def get_nearest_node(node_list, random_node):
    return min(node_list, key=lambda node: distance(node, random_node))


# @profile(precision=4)
def greedy_rrt_frontier_detect(image_path, start, passwall=False, max_iter=8000):
    gc.disable()
    # 加载图片
    image = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
    height, width = image.shape
    # 生成图
    color_image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)

    # 开始节点
    start_node = Node(start[0], start[1])

    # node列表
    node_list = [start_node]
    # 边界列表
    frontier_list = []
    cv2.circle(color_image, (start_node.x, start_node.y), 1, (0, 0, 255), -1)  # 半径为1，颜色为绿色，-1表示填充

    # 方向向量
    directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]

    # 定义灰色和白色的阈值
    gray_value_low = 100
    gray_value_top = 240
    white_value_low = 249
    white_value_top = 255

    # 迭代总次数
    num = 0
    # 重复点数量
    # xxxxx = 0
    # 定义搜索边长为5的方框区域，半径为2
    search_radius = 0

    # 定义node的邻域大小
    search_node_radius = 2

    for _ in range(max_iter):
        # 统计总次数
        num = num + 1
        # print(num)
        # 生成随机点
        random_node = get_random_node(width, height)
        # 获取最近的节点
        nearest_node = get_nearest_node(node_list, random_node)

        # 生成new节点
        theta = math.atan2(random_node.y - nearest_node.y, random_node.x - nearest_node.x)
        new_node = Node(
            int(nearest_node.x + 10 * math.cos(theta)),
            int(nearest_node.y + 10 * math.sin(theta))
        )
        # 防止溢出
        new_node.x = min(new_node.x, width - 1)
        new_node.x = max(new_node.x, 0)
        new_node.y = min(new_node.y, height - 1)
        new_node.y = max(new_node.y, 0)
        new_node.parent = nearest_node
        # 画点
        cv2.circle(color_image, (new_node.x, new_node.y), 1, (0, 0, 0), -1)  # 半径为1，颜色为绿色，-1表示填充
        # 画线
        cv2.line(color_image, (nearest_node.x, nearest_node.y), (new_node.x, new_node.y),
                 (0, 255, 0), 1)

        # 检查是否是边界（以new_node为中心的5x5方框）(检测八邻域内的所有点是否满足)
        # flag = False
        # for dy in range(-search_radius, search_radius + 1):
        #     for dx in range(-search_radius, search_radius + 1):
        #         check_x = max(new_node.x + dx, 0)
        #         check_x = min(check_x, width - 1)
        #         check_y = max(new_node.y + dy, 0)
        #         check_y = min(check_y, height - 1)
        #         if white_value_low <= image[check_y, check_x] <= white_value_top and any(
        #                 gray_value_low <= image[check_y + ddy, check_x + ddx] <= gray_value_top for ddx, ddy in
        #                 directions if
        #                 0 <= check_x + ddx < width and 0 <= check_y + ddy < height):
        #             new_node.x = check_x
        #             new_node.y = check_y
        #             frontier_list.append(new_node)
        #             cv2.circle(color_image, (new_node.x, new_node.y), 3, (0, 0, 255), -1)  # 半径为1，颜色为绿色，-1表示填充
        #             flag = True
        #             break
        #     if flag:
        #         break
        # 检查是否是边界（以new_node为中心的5x5方框）(检测该点24邻域是否满足)
        flag = False
        for dy in range(-search_node_radius, search_node_radius + 1):
            for dx in range(-search_node_radius, search_node_radius + 1):
                check_x = max(new_node.x + dx, 0)
                check_x = min(check_x, width - 1)
                check_y = max(new_node.y + dy, 0)
                check_y = min(check_y, height - 1)
                # print(new_node.x, new_node.y)
                if white_value_low <= image[new_node.y, new_node.x] <= white_value_top and gray_value_low <= image[
                    check_y, check_x] <= gray_value_top:
                    # if white_value_low <= image[new_node.y, new_node.x] <= white_value_top:
                    # print(new_node.x, new_node.y)
                    # if gray_value_low <= image[check_y, check_x] <= gray_value_top:
                    # 查找这个灰色点的相邻的白色区域（定义白色部分是边界）
                    is_exist = False
                    for dxx, dyy in directions:
                        if white_value_low <= image[check_y + dyy, check_x + dxx] <= white_value_top and 0 <= check_x + dxx < width and 0 <= check_y + dyy < height:
                            check_x = check_x + dxx
                            check_y = check_y + dyy
                            is_exist = True
                            break
                    if not is_exist:
                        continue
                    # 查找属于哪一个点的四领域
                    new_node.x = check_x
                    new_node.y = check_y
                    # if new_node in frontier_list:
                    #     xxxxx = xxxxx + 1
                    if new_node not in frontier_list:
                        frontier_list.append(new_node)
                    else:
                        pass
                        # print("重复")
                    # frontier_list.append(new_node)
                    # 查找到相邻的所有边界
                    find_connected_frontier(image, new_node, color_image, frontier_list, directions,
                                            gray_value_low, gray_value_top, width, height, white_value_low,
                                            white_value_top)

                    cv2.circle(color_image, (new_node.x, new_node.y), 3, (0, 0, 255), -1)  # 半径为1，颜色为绿色，-1表示填充
                    flag = True
                    break
            if flag:
                break
        if passwall:
            node_list.append(new_node)
        elif image[new_node.y, new_node.x] >= 249:  # 不能是障碍物
            node_list.append(new_node)

    # print("探测到的边界总数:")
    # print(len(frontier_list))
    # print([(node.x, node.y) for node in frontier_list])  # 输出边界点的坐标
    # color_image1 = cv2.resize(color_image, None, fx=0.3, fy=0.1)
    # cv2.imshow('greedy-RRT frontier', color_image1)
    # cv2.waitKey(0)
    # cv2.destroyAllWindows()
    # print("重复数量",xxxxx)
    return len(frontier_list)


def find_connected_frontier(image, start_node, color_image, frontier_list, directions, gray_value_low, gray_value_top,
                            width, height, white_value_low, white_value_top):
    # 八邻域
    directions_8 = [(-1, 0), (1, 0), (0, -1), (0, 1), (-1, -1), (1, 1), (1, -1), (-1, 1)]
    # 创建一个队列，用于广度优先搜索
    q = queue.Queue()
    q.put(start_node)

    # 标记该节点为已访问
    visited = set()
    visited.add((start_node.x, start_node.y))

    # 开始广度优先搜索
    while not q.empty():
        node = q.get()

        # 将当前节点添加到frontier_list并在图片上标记
        # frontier_list.append(copy.deepcopy(node))  # 需要深拷贝节点，避免后续修改影响
        cv2.circle(color_image, (node.x, node.y), 3, (0, 0, 255), -1)

        # 遍历该节点的所有相邻节点
        for dy in range(-1, 2):  # 检查上下左右以及对角线邻域 (-1, 0, 1)
            for dx in range(-1, 2):
                if dx == 0 and dy == 0:
                    continue  # 跳过自身
                check_x = max(min(node.x + dx, width - 1), 0)
                check_y = max(min(node.y + dy, height - 1), 0)
                if 0 <= check_x < width and 0 <= check_y < height:
                    # 如果该点符合边界条件，并且没有访问过
                    if ((white_value_low <= image[check_y, check_x] <= white_value_top)
                            and (check_x, check_y) not in visited
                            and any(
                                gray_value_low <= image[check_y + ddy, check_x + ddx] <= gray_value_top for ddx, ddy
                                in
                                directions if 0 <= check_x + ddx < width and 0 <= check_y + ddy < height)):
                        new_node = Node(check_x, check_y)
                        new_node.parent = node  # 设置父节点，便于路径回溯

                        # 将该点标记为已访问，并加入队列
                        visited.add((new_node.x, new_node.y))
                        q.put(new_node)
                        # if new_node in frontier_list:
                        #     xxxxx = xxxxx + 1
                        if new_node not in frontier_list:
                            frontier_list.append(new_node)
                        else:
                            pass
                            # print("重复")
                        # frontier_list.append(new_node)
                        # print("确实存在连续")
    return frontier_list


if __name__ == '__main__':
    # 使用该函数
    greedy_rrt_frontier_detect('path_to_your_image.jpg', (600, 520))
